#include "data_scene_impl.h"

#include <algorithm>
#include <future>
#include <mutex>
#include <opencv2/opencv.hpp>

#include "file_helper.h"
#include "json_helper.h"

namespace autodrive {
namespace perception {

DataSceneImpl::~DataSceneImpl() { is_init_ = false; }

void DataSceneImpl::Init(std::filesystem::path const &path) noexcept(false) {
  path_ = path;
  auto initFunc{[this](std::filesystem::path const &scenePath) {
    if (sync_info_files_ptr_ == nullptr) {
      sync_info_files_ptr_ =
          std::make_shared<DataScene::DataFrameArray<std::filesystem::path>>();
    }

    if (data_frame_keys_ptr_ == nullptr) {
      data_frame_keys_ptr_ =
          std::make_shared<DataScene::DataFrameArray<std::string>>();
    }

    if (nullptr == calib_data_ptr_) {
      calib_data_ptr_ =
          std::make_shared<DataScene::CalibDataArray<DataScene::MatrixData>>();
    }

    std::string const &metaDataFileName{"metadata.json"};
    auto const &metaDataFilePath{scenePath /
                                 std::filesystem::path(metaDataFileName)};
    meta_data_ = read_meta_data(metaDataFilePath);

    std::string const &syncInfoPathName{"sync_info"};
    auto const &syncInfoPath{scenePath /
                             std::filesystem::path(syncInfoPathName)};

    travel_timestamp_data_file(syncInfoPath, sync_info_files_ptr_);
    parse_sync_info_dict(sync_info_files_ptr_, sync_info_dict_,
                         data_frame_keys_ptr_);

    std::string const &calibPathName{"calib"};
    auto const &calibPath{scenePath / std::filesystem::path(calibPathName)};
    load_calib_data(calibPath, calib_data_ptr_);
  }};
  std::call_once(init_flag_, initFunc, path);
  is_init_ = true;
}

DataScene::MetaDataInfo DataSceneImpl::read_meta_data(
    std::filesystem::path const &file) const noexcept(false) {
  if (!std::filesystem::exists(file)) {
    throw std::runtime_error(file.string() + " does not exist");
  }

  auto const &jStr{read_text(file)};

  boost::json::value jv;

  try {
    boost::json::error_code ec;
    jv = boost::json::parse(jStr, ec);
    if (ec.value() != 0) {
      throw std::runtime_error("parse meta info json string fail: " +
                               ec.message());
    }
  } catch (const std::exception &e) {
    throw std::runtime_error("parse json string fail: " +
                             std::string(e.what()));
  } catch (...) {
    throw std::runtime_error("parse meta info json string fail, unexpected");
  }

  DataScene::MetaDataInfo info;

  auto const &joRoot{jv.as_object()};
  json_extract(joRoot, "name", info.name);
  json_extract(joRoot, "description", info.description);
  json_extract(joRoot, "tags", info.tags);
  json_extract(joRoot, "lidar_channels", info.lidar_channels);
  json_extract(joRoot, "radar_channels", info.radar_channels);
  json_extract(joRoot, "main_sensor", info.main_sensor);
  json_extract(joRoot, "weather", info.weather);
  json_extract(joRoot, "lighting", info.lighting);
  json_extract(joRoot, "road_condition", info.road_condition);
  json_extract(joRoot, "road_type", info.road_condition);
  return info;
}

void DataSceneImpl::parse_sync_info_dict(
    DataScene::DataFrameArrayPtr<std::filesystem::path> const
        &sync_info_files_ptr,
    DataSceneImpl::SyncInfoDict &sync_info_dict,
    DataScene::DataFrameArrayPtr<std::string> &key_array_ptr) const
    noexcept(false) {
  if (nullptr == sync_info_files_ptr) {
    throw std::runtime_error("sync_info_files_ptr is nullptr");
  }

  if (nullptr == key_array_ptr) {
    throw std::runtime_error("key_array_ptr is nullptr");
  }

  sync_info_dict.clear();
  key_array_ptr->clear();

  for (auto const &syncInfoFile : *sync_info_files_ptr) {
    auto const &syncInfo{parse_sync_info(syncInfoFile)};
    auto const &key{syncInfoFile.filename().stem()};
    sync_info_dict.emplace(key, syncInfo);
    key_array_ptr->add(key);
  }
}

DataSceneImpl::SyncInfo DataSceneImpl::parse_sync_info(
    std::filesystem::path const &file_path) const noexcept(false) {
  if (!std::filesystem::exists(file_path)) {
    throw std::runtime_error(file_path.string() + " does not exist");
  }
  auto const &jStr{read_text(file_path)};
  return json_deserialize<SyncInfo>(jStr);
}

void DataSceneImpl::travel_timestamp_data_file(
    std::filesystem::path const &root,
    DataScene::DataFrameArrayPtr<std::filesystem::path> files_ptr) const
    noexcept(false) {
  if (!std::filesystem::exists(root)) {
    throw std::runtime_error(root.string() + " does not exist");
  }

  if (nullptr == files_ptr) {
    throw std::runtime_error("files_ptr is nullptr");
  }

  files_ptr->clear();

  std::filesystem::directory_iterator rootIter(root);
  for (auto const &data : rootIter) {
    if (files_ptr->full()) {
      throw std::runtime_error("sync info file number is larger than: " +
                               std::to_string(DataScene::kMaxDataFrameNumber));
    }
    files_ptr->add(data.path());
  }

  std::sort(
      files_ptr->begin(), files_ptr->end(),
      [](std::filesystem::path const &lp, std::filesystem::path const &rp) {
        auto lt = std::strtoull(lp.filename().stem().c_str(), 0, 0);
        auto rt = std::strtoull(rp.filename().stem().c_str(), 0, 0);
        return lt <= rt;
      });
}

void DataSceneImpl::load_calib_data(
    std::filesystem::path const &path,
    DataScene::CalibDataArrayPtr<DataScene::MatrixData> calib_data_ptr) const
    noexcept(false) {
  if (!std::filesystem::exists(path)) {
    throw std::runtime_error(path.string() + " does not exist");
  }

  if (nullptr == calib_data_ptr) {
    throw std::runtime_error("data_calib_ptr is nullptr");
  }

  calib_data_ptr->clear();
  std::filesystem::directory_iterator rootIter(path);
  for (auto const &calibFile : rootIter) {
    std::vector<double> data;
    read_text(calibFile, data);
    if (data.size() > DataScene::kMaxMatrixDataSize) {
      throw std::runtime_error(
          "calib file " + calibFile.path().string() +
          " is illegal, data size=" + std::to_string(data.size()) +
          " is large than " + std::to_string(DataScene::kMaxMatrixDataSize));
    }
    DataScene::MatrixData matrixData;
    matrixData.name = calibFile.path().filename().stem();
    matrixData.is_valid = true;
    matrixData.timestamp = 0;
    matrixData.raw_data.clear();
    for (auto const &d : data) {
      matrixData.raw_data.add(d);
    }
    calib_data_ptr->add(matrixData);
  }
}

DataSceneImpl::SyncInfoData DataSceneImpl::parse_sync_info_data(
    std::string const &key, std::string const &catagory) const noexcept(false) {
  auto const &syncInfoPair{sync_info_dict_.find(key)};
  if (syncInfoPair == sync_info_dict_.end()) {
    throw std::runtime_error("no " + key + " sync info");
  }
  auto const &syncInfo{syncInfoPair->second};
  auto const &cataInfoPair{syncInfo.find(catagory)};

  if (cataInfoPair == syncInfo.end()) {
    throw std::runtime_error("no " + catagory + " sync info");
  }
  return cataInfoPair->second;
}

void DataSceneImpl::LoadCameraData(
    std::vector<std::string> const &cameraNames, std::string const &key,
    DataScene::CameraDataArray &cameraDataArray) const noexcept(false) {
  if (!IsInit()) {
    throw std::runtime_error("current data scene is not init");
  }
  auto const &cameraKey{"cameras"};
  auto const &cameraSyncInfo{parse_sync_info_data(key, cameraKey)};

  auto cameras{cameraNames.size()};
  if (cameras > DataScene::kMaxCameraNum) {
    throw std::runtime_error(
        "camera to read number=" + std::to_string(cameras) +
        " is larger than kMaxCameraNum=" +
        std::to_string(DataScene::kMaxCameraNum));
  }
  cameraDataArray.resize(cameras);

  auto readCameraDataFunc{
      [this, &cameraSyncInfo](std::string const &cameraName,
                              DataScene::CameraData &cameraData) {
        cameraData.name = cameraName;
        cameraData.is_valid = false;
        cameraData.timestamp = 0;

        auto const &filePair{cameraSyncInfo.find(cameraName)};
        if (filePair == cameraSyncInfo.end()) {
          throw std::runtime_error("no camera " + cameraName + " data found");
        }
        cameraData.timestamp = filePair->second;
        auto const &filePath{
            Path() / std::filesystem::path(cameraKey) /
            std::filesystem::path(filePair->first) /
            std::filesystem::path(std::to_string(filePair->second) + ".jpg")};
        if (!std::filesystem::exists(filePath)) {
          throw std::runtime_error(filePath.string() + " does not exist");
        }
        cameraData.image = cv::imread(filePath);
        cameraData.is_valid = true;
      }};
  std::vector<std::shared_future<void>> futureList;
  for (std::int32_t i{0}; i < cameras; ++i) {
    auto const &name{cameraNames.at(i)};
    auto &cameraData{cameraDataArray.at(i)};
    // readCameraDataFunc(name, cameraData);
    std::shared_future<void> f{std::async(std::launch::async,
                                          readCameraDataFunc, std::ref(name),
                                          std::ref(cameraData))};
    futureList.emplace_back(f);
  }

  bool hasError{false};
  std::string errMsg{"read camera data error, "};

  for (auto &future : futureList) {
    try {
      future.get();
    } catch (const std::exception &e) {
      errMsg += std::string(e.what()) + " ";
      hasError = true;
    }
  }

  if (hasError) {
    throw std::runtime_error(errMsg);
  }
}

void DataSceneImpl::LoadLidarData(
    std::vector<std::string> const &lidarNames, std::string const &key,
    DataScene::LidarDataArray &lidarDataArray) const noexcept(false) {
  if (!IsInit()) {
    throw std::runtime_error("current data scene is not init");
  }
  auto const &lidarKey{"lidars"};
  auto const &lidarSyncInfo{parse_sync_info_data(key, lidarKey)};

  lidarDataArray.clear();
  for (auto const &name : lidarNames) {
    auto const &filePair{lidarSyncInfo.find(name)};
    if (filePair == lidarSyncInfo.end()) {
      throw std::runtime_error("no lidar " + name + " data found");
    }

    if (lidarDataArray.full()) {
      throw std::runtime_error("lidar data array is full");
    }

    auto const &filePath{
        Path() / std::filesystem::path(lidarKey) /
        std::filesystem::path(filePair->first) /
        std::filesystem::path(std::to_string(filePair->second) + ".bin")};

    auto lidarDataPtr{lidarDataArray.end()};
    read_lidar_data(filePath, meta_data_.lidar_channels,
                    lidarDataPtr->point_cloud);
    lidarDataPtr->timestamp = filePair->second;
    lidarDataPtr->name = name;
    lidarDataPtr->is_valid = true;
    lidarDataArray.resize(lidarDataArray.size() + 1);
  }
}

void DataSceneImpl::read_lidar_data(
    std::filesystem::path const &path, std::vector<std::string> const &channels,
    DataScene::LidarPointCloud &point_cloud) const noexcept(false) {
  if (!std::filesystem::exists(path)) {
    throw std::runtime_error(path.string() + " does not exist");
  }

  if (channels.empty()) {
    throw std::runtime_error("lidar channels is empty");
  }

  std::vector<float> pclData;
  read_bin(path, pclData);

  auto channelNumber{channels.size()};
  auto pointNumber{pclData.size() / channelNumber};

  if (pointNumber > DataScene::kMaxPointNum) {
    throw std::runtime_error(
        path.string() + ", file data number: " + std::to_string(pointNumber) +
        " is larger than max point number: " +
        std::to_string(DataScene::kMaxPointNum));
  }

  auto xIndex{find_index(channels, std::string("x"))};
  if (xIndex == invalidIndex) {
    throw std::runtime_error("no x channel found in lidar data");
  }

  auto yIndex{find_index(channels, std::string("y"))};
  if (yIndex == invalidIndex) {
    throw std::runtime_error("no y channel found in lidar data");
  }

  auto zIndex{find_index(channels, std::string("z"))};
  if (zIndex == invalidIndex) {
    throw std::runtime_error("no z channel found in lidar data");
  }

  auto intensityIndex{find_index(channels, std::string("intensity"))};
  auto timestampIndex{find_index(channels, std::string("timestamp"))};

  point_cloud.clear();
  for (std::int32_t i = 0; i < pointNumber; ++i) {
    DataScene::PointXYZIT p{.x = pclData.at(i * channelNumber + xIndex),
                            .y = pclData.at(i * channelNumber + yIndex),
                            .z = pclData.at(i * channelNumber + zIndex)};
    if (intensityIndex != invalidIndex) {
      p.intensity = pclData.at(i * channelNumber + intensityIndex);
    }
    if (timestampIndex != invalidIndex) {
      p.timestamp = pclData.at(i * channelNumber + timestampIndex);
    }

    point_cloud.add(p);
  }
}

void DataSceneImpl::LoadRadarData(
    std::vector<std::string> const &radarNames, std::string const &key,
    DataScene::radarDataArray &radarDataArray) const noexcept(false) {
  /// todo
}

void DataSceneImpl::LoadEgoMotionData(std::string const &key,
                                      DataScene::MatrixData &egoData) const
    noexcept(false) {
  if (!IsInit()) {
    throw std::runtime_error("current data scene is not init");
  }
  auto const &egoMotionKey{"ego_motion"};
  auto const &filePath{Path() / std::filesystem::path(egoMotionKey) /
                       std::filesystem::path(key + ".txt")};

  std::vector<double> egoRawData;
  read_text(filePath, egoRawData);

  if (egoRawData.size() > DataScene::kMaxMatrixDataSize) {
    throw std::runtime_error(
        filePath.string() +
        ", ego data size=" + std::to_string(egoRawData.size()) +
        ", is large than: " + std::to_string(DataScene::kMaxMatrixDataSize));
  }

  egoData.raw_data.clear();
  for (auto const &d : egoRawData) {
    egoData.raw_data.add(d);
  }
  egoData.name = key;
  egoData.timestamp = std::strtoull(key.c_str(), 0, 0);
  egoData.is_valid = true;
}

}  // namespace perception
}  // namespace autodrive